import ctypes
import sys
import time
import logging



from config import CODE_fi, HOST_fi, CODE_se, HOST_se, last_set, SPEED
from robotic_arm import Arm, SerCom, SockCom


logging.basicConfig(filename='workbench.log', level=logging.DEBUG,
                    format='%(asctime)s %(levelname)s: %(message)s')
console = logging.StreamHandler()
console.setLevel(logging.INFO)
formatter = logging.Formatter('%(asctime)s %(levelname)s: %(message)s')
console.setFormatter(formatter)
logging.getLogger('').addHandler(console)


def change_camera(sock,tag):
    """
    切换当前照相机的状态
    :return:
    """

    sock.send(tag)

def all_connection():
    """
    串口 socket 机械臂 建立连接
    :return:
    """

    fi_arm = Arm(CODE_fi,HOST_fi)
    se_arm = Arm(CODE_se,HOST_se) #++第二个机械臂的code和host

    ser = SerCom('COM8',38400)    #串口连接的路径和波特率
    sock = SockCom('192.168.1.66',6000)


    return ser,sock,fi_arm,se_arm

def equipment_init(fi_arm,ser,se_arm):
    """
    相应设备位置的初始化：

    1.相机提前切换到 erweima模式  #这个动作取消，现在只需要 发送一个 指令 'paizhao'
    2.第一个机械臂上电并打开夹爪
    3.转盘转动
    4.第一个机械臂移动到初始位置
    5.第二个机械臂移动到初始位置
    6.中央导轨初始化到初始位置

    """

    fi_arm.open_draw(3,1) #电压输出类型24v 以及 block
    fi_arm.release(700,1)#手爪打开
    turn(ser)
    first_rail(fi_arm, 300, 20, 1)#升降导轨运行至300mm
    time.sleep(1.5)
    first_rail(fi_arm,232,SPEED,1)     #第一个导轨位置初始化


    se_arm.updown([22.061, -6.539, 93.422, -0.019, 93.147, -36.367],SPEED,0,1)   #视觉机械臂移动到二维码的上方(初始位置)movej
    second_rail(se_arm,fi_arm,0)  #中央导轨位置初始化 等待验证  靠近左臂  是左臂第三通道控制



def first_rail(fi_arm,height,speed,block):
    """
    第一个导轨移动到指定位置
    :return:
    """
    logging.info('第一个机械臂导轨初始化位置')
    a = fi_arm.chassis_movement(height,speed,block)
    logging.info(f'第一个机械臂导轨初始化完毕：{a}')

    return a

def second_arm(se_arm,height,speed,block):
    """
    第二个导轨移动到指定位置
    :return:
    """
    logging.info('第二个机械臂导轨初始化位置')
    a = se_arm.chassis_movement(height,speed,block)
    logging.info(f'第二个机械臂导轨初始化完毕：{a}')

    return a



def second_rail(se_arm,fi_arm,label):
    """
    中央导轨移动到两端处 高电压靠近右臂 低电压靠近左臂
    **中央导轨从第二个机械臂移动到第一个机械臂处 此时 使用者给它一个低电平（输出），如果它到达第一个机械臂处，此时中央导轨输入一个低电平

    输入和输出都是相对于  使用者而言**
    """
    if label:
        logging.info(f'中央导轨向右臂移动：{label}')
        print(f'中央导轨向右臂移动：{label}')

        fi_arm.central_rail(0, 1, True, 1)  # 中央导轨的移动准备拍摄二维码，如果到达右臂处，数字的输入状态变为0 chu io,num,state,block
        fi_arm.io_judgment(label)  # 是否在整个流程开始时 让中央导轨初始化位于第一个机械臂处

    else:
        logging.info(f'中央导轨向左臂移动：{label}')
        print(f'中央导轨向左臂移动：{label}')

        fi_arm.central_rail(0, 1, False, 1)  # 中央导轨的移动初始化，如果到达初始位置，数字的输入状态变为0 io,num,state,block
        time.sleep(1)
        fi_arm.io_judgment(label)  # 是否在整个流程开始时 让中央导轨初始化位于第一个机械臂处


def turn(ser):
    """
    2-4次转动转盘
    :return:
    """

    ser.send() #发送命令旋转90°



def chassis_movement(fi_arm):
    """
    第一段
    第一个机器臂移动导轨并抓取物料盘放在中央导轨
    :return:
    """
    logging.info(f"第一个机械臂开始恢复到初始位")
    fi_arm.updown([-72.835, -2.483, 79.876, -0.527, 102.359, -72.805],SPEED,0,1) #第一个机械臂移动到初始位置
    logging.info(f"第一个机械臂恢复到初始位")


    logging.info(f"第一个机械臂移动到转盘处") #动作2

    fi_arm.updown([-84.305, 46.763, 24.308, -0.344, 108.343, -81.982],SPEED, 0, 1)#第一个机械臂移动到转盘上方moveJ移动
    # fi_arm.horizontal([-0.042299, 0.371029, 0.29951, -3.095, -0.003, -0.063],SPEED,0,1)#第一个机械臂向转盘下方抓取点movel移动
    fi_arm.horizontal([-0.04234, 0.385619, 0.29707, -3.13, -0.004, -0.041], SPEED, 0, 1)


    logging.info(f"第一个机械臂成功移动到转盘处")

    fi_arm.clamping(750,750,1) #speed,force,block
    time.sleep(1)



    logging.info(f"第一个机械臂转向中央导轨")#动作3

    # fi_arm.horizontal([-0.03702, 0.372429, 0.33098, -3.095, -0.004, -0.063],20,0,1)

    # fi_arm.horizontal([-0.04274, 0.36763, 0.37222, -3.096, -0.004, -0.063],SPEED,0,1) #第一个机械臂抓取后moveL移动到转盘上方(之前抓取点太高出现过问题)
    fi_arm.horizontal([-0.04236, 0.38497, 0.353209, 3.151, -0.004, -0.043], SPEED, 0, 1)

    # fi_arm.updown([-42.433, 8.319, 69.4, -2.089, 101.583, -41.748],SPEED,0,1)#第一个机械臂movej移动到中央导轨上方
    fi_arm.updown([-46.78, 8.914, 72.81, -0.572, 98.066, -46.813], SPEED, 0, 1)

    # fi_arm.horizontal([-0.17555, 0.17077, 0.325609, -3.107, -0.014, -0.019],SPEED,0,1)#第一个机械臂movel移动到中央导轨抓取点
    fi_arm.horizontal([-0.168789, 0.18172, 0.321589, 3.151, -0.004, -0.001], SPEED, 0, 1)

    fi_arm.release(500,1)

    logging.info('将物料盘房放在中央导轨处')


    time.sleep(1) #不确定夹爪张开过程所需要的时间


    logging.info(f"第一个机械臂开始恢复到初始位") #动作4
    # fi_arm.horizontal([-0.176589, 0.16841, 0.39457, -3.107, -0.014, -0.019],SPEED,0,1) #第一个机械臂movel移动到中央导轨上方
    fi_arm.horizontal([-0.168789, 0.18172, 0.427589, 3.151, -0.004, -0.001] , SPEED, 0, 1)
    # fi_arm.updown([-60.36, -3.921, 83.654, -0.035, 97.876, -58.955],SPEED,0,1)#第一个机械臂movej移动到初始点
    fi_arm.updown([-72.837, -2.483, 79.878, -0.527, 102.359, -72.805], SPEED, 0, 1)

    logging.info('第一个机械臂恢复到初始位置')

def identify(fi_arm,se_arm,sock,freq):
    """
    第二段
    第二个机械臂加上吸盘工具识别材料盘上二维码的颜色
    :return:识别到的颜色
    """


    second_rail(se_arm,fi_arm,1) #state靠近右臂为低电平

    logging.info('开始发送paizhao指令识别二维码')

    sock.send('paizhao')

    result = sock.recv()

    logging.info('paizhao指令处理完毕')

    if not freq: #第一次 需要吸取工具 其余次数不需要吸取

        logging.info('第二个机械臂开始移动至工具架处')

        se_arm.updown([90.91, 21.059, 66.939, 0.495, 91.719, -58.261],SPEED,0,1)#第二个机械臂movej移动到工具架上方
        # se_arm.updown([91.074, 25.761, 90.501, 0.55, 64.961, -62.099],20,0,1)

        se_arm.central_rail(0,3,True,1) #装备工具 快换工具缩进

        time.sleep(1)

        se_arm.horizontal([0.006019, -0.302519, 0.10679, 3.141, 0.009, 2.604],SPEED,0,1)#第二个机械臂movel移动到吸笔处

        logging.info('第二个机械臂成功移动至工具架处')


        logging.info('第二个机械臂开始装备吸笔')

        se_arm.central_rail(0,3,False,1) # 快换工具张开

        logging.info('第二个机械臂装备吸笔完毕')


        logging.info('第二个机械臂开始转移至物料上方')

        se_arm.horizontal([0.006039, -0.30251, 0.342739, 3.141, 0.009, 2.604],SPEED,0,1)#第二个机械臂movel移动到工具架上方
        # se_arm.updown([-40.755, 3.548, 88.917, -0.008, 86.097, -100.831],20,0,1)
        se_arm.updown([-40.515, 3.789, 88.68, -0.028, 86.109, -99.457], SPEED, 0, 1)  # 第二个机械臂movej移动到直筒拍照点

        logging.info('第二个机械臂转移至物料上方')

    return result[0]

def picking(fi_arm,se_arm,sock,material,freq):
    """
    第三段 找到目标颜色的木块并且把它放到物料盘上
    :param material: 目标颜色
    :return:
    """


    se_arm.updown([-40.515, 3.789, 88.68, -0.028, 86.109, -99.457], SPEED, 0, 1)  # 第二个机械臂movej移动到物料拍照点

    se_arm.central_rail(0, 2, 1, 1)  # 拉杆推出新的木块
    time.sleep(1.5)
    se_arm.central_rail(0, 2, 0, 1)  # 拉杆缩进去

    se_arm.is_suo()
    sock.send('paizhao')
    result = sock.recv()





    while result[0] != material:

        # pose = result[1:3] + [0.394929, 3.135, -0.008, 1.052]
        pose = result[1:3] + [0.400279, 3.137, -0.008, 1.052]#(x，y的值在robotic_arm.py中进行查看或修改)，rx代表木块上方的高度，后面的数值代表z,rx,ry,rx


        print(f'pose:{pose}')

        se_arm.horizontal(pose, SPEED, 0, 1)  # 机械臂2移动到吸取点正上方

        print('移动到正上方')

        se_arm.horizontal(pose[:2] + [0.239039, 3.137, -0.008, 1.052], SPEED, 0, 1)#z代表木块的高度，后面数值代表z,rx,ry,rz

        print('开始吸取！')
        se_arm.central_rail(0, 4, 1, 1)

        se_arm.horizontal([-0.26198, 0.14836, 0.400279, 3.137, -0.008, 1.052], SPEED, 0, 1)  # 机械臂2吸取好移动到上方点

        # se_arm.horizontal([-0.25183, 0.14271, 0.410299, -3.105, -0.004, 1.047], 20, 0, 1)  # 原地旋转


        se_arm.horizontal([-0.26035, 0.24107, 0.387049, 3.138, -0.008, result[3] + 1.052], SPEED, 0, 1)  #机械臂2吸取好放置到直筒中
        se_arm.central_rail(0, 4, 0, 1)  # 放开木块

        # se_arm.updown([-40.755, 3.548, 88.918, -0.009, 86.097, -100.831], 20, 0, 1)  # 拍照点

        se_arm.updown([-40.515, 3.789, 88.68, -0.028, 86.109, -99.457], SPEED, 0, 1)  # 机械臂2返回到直筒(物料仓)拍照点

        se_arm.central_rail(0, 2, 1, 1)  # 拉杆推出新的木块  state不确定 True 或 False 现场测试
        time.sleep(1.5)
        se_arm.central_rail(0, 2, 0, 1)  # 拉杆推出新的木块  state不确定 True 或 False 现场测试

        sock.send('paizhao')
        result = sock.recv()

    pose = result[1:3] + [0.394929, 3.135, -0.008, 1.052]#(x，y的值在robotic_arm.py中进行查看或修改)，后面的数值代表z,rx,ry,rz

    print(f'pose:{pose}')

    se_arm.horizontal(pose, SPEED, 0, 1)  # 机械臂2移动到正上方

    logging.info('移动到正上方')

    se_arm.horizontal(pose[:2] + [0.239039, 3.137, -0.008, 1.052], SPEED, 0, 1)#吸取基准点，后面的数值代表z，rx，ry,rz

    logging.info('开始吸取！')
    se_arm.central_rail(0, 4, 1, 1)

    se_arm.horizontal([-0.25976, 0.144509, 0.3002, 3.138, -0.008, 1.052], SPEED, 0, 1)  # 机械臂2吸取好移动到上方点
    se_arm.updown([18.711, 11.534, 71.319, -0.014, 97.775, -41.603],SPEED,0,1) #机械臂2移动到中央导轨 物料盘上方


    se_arm.horizontal([-0.240009, -0.083519,0.40181,3.133,-0.010,1.052 + result[3]],SPEED,0,1) #机械臂2原地旋转

    se_arm.horizontal([-0.244339, -0.082709, 0.29867, 3.134, -0.008, 1.052 + result[3]],SPEED,0,1) #机械臂2直线移动接近物料盘




    se_arm.central_rail(0,4,0,1)  #放开木块
    se_arm.horizontal([-0.240009, -0.083519, 0.35048, 3.132, -0.009, 1.052 + result[3]],SPEED,0,1)
    # se_arm.updown([27.19, -7.572, 97.458, -0.453, 91.759, -33.661],SPEED,0,1)   #机械臂2回到初始位

    se_arm.updown([-44.755, 7.372, 80.74, -1.162, 90.734, -44.782], SPEED, 0, 1)

    second_rail(se_arm,fi_arm,0) #中央导轨移动靠近左臂为低电平/




def last_setdown(fi_arm,freq):

    """
    机械臂左臂将 中央导轨上的物料放置在指定位置
    """
    first_rail(fi_arm, 400, SPEED, 1)  # 第一个导轨移动到 放置四个物料的最终位置

    # fi_arm.coor_motion(pose,v,r,block)
    # fi_arm.updown([-60.042, 0.805, 76.392, 0.001, 100.8, -59.993], 20, 0, 1)  # 放料盘初始点

    # fi_arm.updown([-104.226, 34.404, 32.854, -1.761, 111.758, -104.922], SPEED, 0, 1)  # 机械臂1转到中央导轨物料盘上方
    fi_arm.updown([-97.332, 28.584, 41.237, -0.819, 109.636, -97.803], SPEED, 0, 1)
    fi_arm.release(500, 1)  # 松开

    # fi_arm.horizontal([0.089779, 0.33, 0.32642, 3.151, -0.032, 0.0], SPEED, 0, 1)  # 机械臂1movel接近物料盘
    fi_arm.horizontal([0.04281, 0.318059, 0.320869, 3.148, -0.014, 0.003], SPEED, 0, 1)
    fi_arm.clamping(750, 750, 1)  # speed,force,block

    fi_arm.horizontal([0.042889, 0.318079, 0.40285, 3.148, -0.014, 0.003], SPEED, 0, 1)  # 机械臂1移动到中央导轨上升

    fi_arm.updown(last_set[freq][0], SPEED, 0, 1)  # 抓住物料盘移动到物料最终位置上方(ctrl+鼠标左键last_set可以查看到机械臂1移动到物料盘的四个位置)
    fi_arm.horizontal(last_set[freq][1], SPEED, 0, 1)  # 接近最终位置
    fi_arm.release(500, 1)  # 松开

    fi_arm.horizontal(last_set[freq][2], 10, 0, 1)  # 上上
#def IO_init():




def main(ser,sock,fi_arm,se_arm,freq):

    equipment_init(fi_arm,ser,se_arm)
    chassis_movement(fi_arm)
    material = identify(fi_arm,se_arm,sock,freq)

    picking(fi_arm,se_arm,sock,material,freq)

    last_setdown(fi_arm,freq)



def shebei_up(se_arm,fi_arm):
    """
    把吸笔放回
    """
    first_rail(fi_arm,300,20,1)     #第一个导轨位置初始化
    time.sleep(1.5)
    fi_arm.updown([-60.36, -3.921, 83.655, -0.035, 97.877, -58.955],SPEED,0,1) #第一个机械臂初始位置

    se_arm.updown([27.19, -7.572, 97.458, -0.453, 91.759, -33.661],SPEED,0,1)   #第二个机械臂初始位置

    logging.info('第二个机械臂开始移动至工具架处')

    se_arm.updown([90.91, 21.059, 66.939, 0.495, 91.719, -58.261], SPEED, 0, 1)#机械臂2移动到工具架位置上方


    se_arm.horizontal([0.006019, -0.302519, 0.10679, 3.141, 0.009, 2.604], SPEED, 0, 1)#机械臂2movel移动到吸笔处

    logging.info('第二个机械臂成功移动至工具架处')

    logging.info('第二个机械臂开始放下吸笔')

    se_arm.central_rail(0, 3, True, 1)  # 快换工具张开



    logging.info('第二个机械臂放下吸笔完毕')

    logging.info('第二个机械臂开始恢复初始位')

    se_arm.horizontal([0.006039, -0.30251, 0.342739, 3.141, 0.009, 2.604], SPEED, 0, 1)#机械臂2movel移动到工具架上方

    se_arm.updown([22.061, -6.539, 93.422, -0.019, 93.147, -36.367],SPEED,0,1)   #机械臂2返回二维码的上方位置(初始位置)

if __name__ == '__main__':

    ser, sock, fi_arm, se_arm = all_connection()

    try:
        for i in range(4):
            main(ser,sock,fi_arm,se_arm,i)
        shebei_up(se_arm,fi_arm)


    except (KeyboardInterrupt,Exception)  as e:
        logging.info(f'出错：{e}')
        sys.exit()






